#include "PL2219.h" 
#include "software_i2c.h"




#include <string.h>
#include <stdio.h>
#include <stdbool.h>
#include "stdlib.h"
 
 
#define  _CLIFF_PIN1    GPIO_PIN_5   //PA1
#define  _CLIFF_PIN2    GPIO_PIN_1   //PA5
#define  _CLIFF_PIN3    GPIO_PIN_7   //PA6
#define  _CLIFF_PIN4    GPIO_PIN_6   //PA7

#define  INIT_ALL_PIN_LEVEL  GPIO_PIN_SET
 
#define  Trig_CLIFF_PIN_LEVEL       GPIO_PIN_SET 

#define  UnTrig_CLIFF_PIN_LEVEL     !Trig_CLIFF_PIN_LEVEL 


static void Set_Out_PIN(uint16_t pins,bool level)  // ��������״̬
{
		HAL_GPIO_WritePin(GPIOA,pins,level == true ? GPIO_PIN_SET : GPIO_PIN_RESET);
}

 
 
 PL2219_t   PL2219 = {
 
	  .PL2219[0]={
		
			.pins = _CLIFF_PIN1,
			.Set_Out = Set_Out_PIN
		},
		.PL2219[1]={
		
			.pins = _CLIFF_PIN2,
			.Set_Out = Set_Out_PIN
		},
		.PL2219[2]={
			.pins = _CLIFF_PIN3,
			.Set_Out = Set_Out_PIN
		},
		.PL2219[3]={
		
			.pins = _CLIFF_PIN4,
			.Set_Out = Set_Out_PIN
		},
		 
	 
 };
 


#define PL2219_COUNT 4

#define STABILITY_THRESHOLD 10            // 稳定性阈值，用于减少噪声干扰
#define SAMPLE_COUNT 10                  // 用于平滑的采样次数
#define STABILITY_TIME_THRESHOLD 100     // 稳定时间阈值，单位为毫秒，用于判断是否稳定（例如悬空状态）
#define MUTATION_THRESHOLD 50            // 突变性阈值，检测数据变化大于该阈值时，认为发生了突变

unsigned long last_stable_time[PL2219_COUNT] = {0};  // 用于记录稳定状态下的时间
unsigned int proximity_samples[PL2219_COUNT][SAMPLE_COUNT];  // 用于存储每一路传感器的采样值
unsigned int sample_index[PL2219_COUNT] = {0};  // 每一路传感器的当前采样位置
unsigned int smoothed_proximity[PL2219_COUNT] = {0};  // 每一路传感器的平滑后的距离值
bool is_deal[PL2219_COUNT] = {false};  // 每一路传感器是否进入Hold On状态
unsigned int last_scale_proximit[PL2219_COUNT] = {0};  // 每一路传感器的最后稳定值

bool is_mutation_detected[PL2219_COUNT] = {false}; // 标记突变是否已被处理
unsigned int mutation_count[PL2219_COUNT] = {0};  // 每个传感器的突变计数器
unsigned long last_mutation_time[PL2219_COUNT] = {0};  // 上次突变发生的时间



// 平滑处理函数，计算采样值的平均值
void add_sample(unsigned int sample, int sensor_id) {
    // 判断是否是第一次采样，如果是，初始化整个缓冲区为当前采样值
    if (sample_index[sensor_id] == 0) {
        for (int i = 0; i < SAMPLE_COUNT; i++) {
            proximity_samples[sensor_id][i] = sample;
        }
    }
    
    // 将当前采样值添加到缓冲区
    proximity_samples[sensor_id][sample_index[sensor_id]] = sample;
    sample_index[sensor_id] = (sample_index[sensor_id] + 1) % SAMPLE_COUNT;

    // 计算当前的平均值
    unsigned int sum = 0;
    for (int i = 0; i < SAMPLE_COUNT; i++) {
        sum += proximity_samples[sensor_id][i];
    }
    smoothed_proximity[sensor_id] = sum / SAMPLE_COUNT;
}

// 判断数据是否稳定
bool is_stable_for_time(int sensor_id) {
    unsigned long current_time = HAL_GetTick();  // 获取当前时间（毫秒）

	#if 1
    // 如果数据在稳定阈值范围内且稳定超过阈值时间，则返回true
    if (abs(smoothed_proximity[sensor_id] - last_scale_proximit[sensor_id]) < STABILITY_THRESHOLD) {
        if (current_time - last_stable_time[sensor_id] > STABILITY_TIME_THRESHOLD) {
            return true;
        }
    } else {
        last_stable_time[sensor_id] = current_time;  // 如果数据变化超过阈值，重置稳定时间
    }
	#else

	   // 如果数据在稳定阈值范围内且稳定超过阈值时间，则返回true
    if (abs(PL2219.PL2219[sensor_id].scale_proximity - last_scale_proximit[sensor_id]) < STABILITY_THRESHOLD) {
        if (current_time - last_stable_time[sensor_id] > STABILITY_TIME_THRESHOLD) {
            return true;
        }
    } else {
        last_stable_time[sensor_id] = current_time;  // 如果数据变化超过阈值，重置稳定时间
    }


	#endif

    return false;
}

// 检测是否为突变性数据
bool is_mutation(int sensor_id) {
    return abs(smoothed_proximity[sensor_id] - last_scale_proximit[sensor_id]) > MUTATION_THRESHOLD;
}


// 检测是否为突变性数据带阈值的
bool is_mutation_thresold(int sensor_id,int thresold,uint8_t *is_positive_or_negative) {

	#if 0
	if((smoothed_proximity[sensor_id] - last_scale_proximit[sensor_id])<0){

			if (is_positive_or_negative != NULL)
			{
				*is_positive_or_negative = 0x00;
			}
			
			
	}else{

			if (is_positive_or_negative != NULL)
			{
				*is_positive_or_negative = 0x01;
			}
	}
		
    return abs(smoothed_proximity[sensor_id] - last_scale_proximit[sensor_id]) > thresold;
	#else

	if((PL2219.PL2219[sensor_id].scale_proximity - last_scale_proximit[sensor_id])<0){

			if (is_positive_or_negative != NULL)
			{
				*is_positive_or_negative = 0x00;
			}
			
			
	}else{

			if (is_positive_or_negative != NULL)
			{
				*is_positive_or_negative = 0x01;
			}
	}
			//printf("id %d: %d\r\n",PL2219.PL2219[sensor_id].id_num,PL2219.PL2219[sensor_id].scale_proximity - last_scale_proximit[sensor_id]);	
    return abs(PL2219.PL2219[sensor_id].scale_proximity - last_scale_proximit[sensor_id]) > thresold;



	#endif
}


							
unsigned int  temp_Image_Avg,temp_Image_Dark,temp_Image_Diff;
unsigned char Addr_0x08_High,Addr_0x08_Low,Addr_0x08_Before,Addr_0x08_Current;
unsigned int  Hover_CNT,No_Hover_CNT;
unsigned char	TH_Diff_H=0x1A; 		//0x10 //0x0F	//0x1A	//0x20 
unsigned char	TH_Diff_L=0x15; 		//0x08 //0x0A	//0x0F	//0x10 
unsigned int	Diff_Pixel0=0x2A;   //0x20 //0x2A //0x60  //80
unsigned int	Diff_Pixel1,Diff_Pixel_Set_TH,Diff_Pixel_Set_TL;
uint8_t flag_Proximity = 0; 				//接近感应标志位
uint8_t Pixel_Adjust_EN = 0; 				//pixel调整使能位
uint8_t	I2C_ACK = 0;
unsigned char I2C_Temp,I_CNT; 



 




/****采集image_dark、image_avg的值，并计算image_dark-image_avg的差值，感应物离传感器越近，差值越大*****/
unsigned int Get_PL2219_MP_Image_Diff(void)
{
	 unsigned int temp0,temp1;
	 unsigned char Image_Avg_H,Image_Avg_L,Image_Dark_H,Image_Dark_L;

	 uint8_t is_busy = 0;
	 wireWriteDataByte(0x00,0xA0); 	 						//Power ON使能传感器               
	 
	 do	{
	 	wireReadDataByte(0x00,&is_busy); 				//等待PL2219-MP芯片运算完成：读取0x00寄存器判断Bit1(Ls_busy)是否等于0
	 }
	 while(is_busy & 0x02);

	 wireWriteDataByte(0x00,0x60); 	  				//Power Down关闭传感器：需等到Ls_busy=0后，才能写Power Down并读取感光数据
	 wireReadDataByte(0x01,&Image_Avg_L);     		//image_avg[7:0]
	 wireReadDataByte(0x02,&Image_Avg_H);     		//image_avg[10:8]||image_dark[10:8]
	 wireReadDataByte(0x03,&Image_Dark_L);    		//image_dark[7:0]
	 Image_Dark_H=Image_Avg_H;	
	 temp0=Image_Avg_H&0x70;     							//image_avg[10:8]
	 temp_Image_Avg=(temp0<<4)+Image_Avg_L;   //image_avg[10:0]
   temp1=Image_Dark_H&0x07;   							//image_dark[10:8]
	 temp_Image_Dark=(temp1<<8)+Image_Dark_L; //image_dark[10:0]

	 //printf("temp_Image_Avg %d,temp_Image_Dark %d ",temp_Image_Avg,temp_Image_Dark);
	 if(temp_Image_Dark>temp_Image_Avg)
	 {
		 temp_Image_Diff=temp_Image_Dark-temp_Image_Avg; //Calculate the difference between image_dark[10:0] and image_avg[10:0]
	 }
	 else
	 {
		 temp_Image_Diff=0;
	 }
	return temp_Image_Diff;	 
 }
#if 0

void PL2219_MP_Image_Dark_Deal(){


}
#else
void PL2219_MP_Image_Dark_Deal()
{
	unsigned char I2C_Temp0,I2C_Temp1;
	Diff_Pixel1=Get_PL2219_MP_Image_Diff(); 	
	
	Addr_0x08_High=0xFF;
	Addr_0x08_Low=0x00;
	wireReadDataByte(0x08,&Addr_0x08_Before);	
	if((temp_Image_Dark<0x400 && Addr_0x08_Before<0xFF) || (temp_Image_Dark>0x7E0 && Addr_0x08_Before>0x00))   //0x400~0x7E0
	{
			Pixel_Adjust_EN=1;
	}
	else 
	{
			Pixel_Adjust_EN=0;
	}
	while(Pixel_Adjust_EN==1)
	{
		 if(temp_Image_Dark<0x400)             	   	   	//0x40
		 {
				 Addr_0x08_Low=Addr_0x08_Before; 	
		 }
		 else if(temp_Image_Dark>0x7E0)            	   	//0x7E 
		 {
				 Addr_0x08_High=Addr_0x08_Before;
		 }
		 I2C_Temp0=Addr_0x08_Low>>1;
		 I2C_Temp1=Addr_0x08_High>>1;
		 Addr_0x08_Current=I2C_Temp1+I2C_Temp0;   
		 if(Addr_0x08_Current!=Addr_0x08_Before)
		 {
					wireWriteDataByte(0x08,Addr_0x08_Current); 
					Addr_0x08_Before=Addr_0x08_Current;
		 }
		 else 
		 {
					if(temp_Image_Dark<0x400 && Addr_0x08_Current<0xFF)
					{
							Addr_0x08_Current=Addr_0x08_Current+1;
					}
					else if(temp_Image_Dark>0x7E0 && Addr_0x08_Current>0x00)
					{
							Addr_0x08_Current=Addr_0x08_Current-1;
					}
					wireWriteDataByte(0x08,Addr_0x08_Current);
					Addr_0x08_Before=Addr_0x08_Current;
					Pixel_Adjust_EN=0;
		 }
		 Diff_Pixel1=Get_PL2219_MP_Image_Diff();					
	}
}
#endif


static uint8_t scale_uint16_to_uint8(uint16_t value) {
    // ����ӳ�乫ʽ�� (value / 65535.0) * 255
    return (uint8_t)((value * 255) / 2048);
}


void PS_Detection_Deal(uint8_t * data,uint16_t * pro_data)
{
		uint8_t scale_diff = 0x00;
		PL2219_MP_Image_Dark_Deal();
		
		scale_diff = scale_uint16_to_uint8(Diff_Pixel1);


		*pro_data = Diff_Pixel1;
		
		*data = scale_diff;

		//printf("PL2219 %d,%d,%d\r\n",scale_diff,Diff_Pixel1,*data);
		// //白色接近
		// if(Diff_Pixel1>Diff_Pixel_Set_TH)  
		// {

		// 	printf("Diff_Pixel1 > H %d\r\n",Diff_Pixel1);
		// 	Hover_CNT++;				 		
		// 	if(Hover_CNT<4)
		// 	{
		// 			for(I_CNT=0;I_CNT<3;I_CNT++)
		// 			{
		// 					HAL_Delay(5);     
		// 					Diff_Pixel1=Get_PL2219_MP_Image_Diff();
		// 					if(Diff_Pixel1>Diff_Pixel_Set_TH)    
		// 							Hover_CNT++; 
		// 					else 
		// 							Hover_CNT=0;  // Hover_CNT--
		// 			}
		// 	}				
		// 	if(flag_Proximity==0 && Hover_CNT>=4)
		// 	{
		// 			flag_Proximity=1; 
		// 			No_Hover_CNT=0;
		// 	} 				
		// }
		// //黑色接近
		// else if( (Diff_Pixel1<Diff_Pixel_Set_TH) && (Diff_Pixel1>Diff_Pixel_Set_TL) ){
			
			
		// 	//处于黑色接近
		// 	printf("Diff_Pixel1 1 %d\r\n",Diff_Pixel1);
		// 	flag_Proximity=1; 
		// 	No_Hover_CNT=0;



		// }
	
		// else if(Diff_Pixel1<Diff_Pixel_Set_TL)
		// {

		// 	printf("Diff_Pixel1 < L %d\r\n",Diff_Pixel1);
		// 	No_Hover_CNT++;       
		// 	if(No_Hover_CNT>=1)   //1/2/4/8	
		// 	{
		// 		flag_Proximity=0;	
		// 		Hover_CNT=0;				
		// 	}			
		// }
		
		// else {
		// 	//处于黑色接近
		// 	printf("Diff_Pixel1 %d\r\n",Diff_Pixel1);
		// 	flag_Proximity=1; 
		// 	No_Hover_CNT=0;
		// }
}



void PS_App_OUT()
{
	if(flag_Proximity==1)
	{
		printf("flag_Proximity\r\n");
		//PS_LED_Status=1;
		//PS_IO_OUT=1;
	}
	else
	{

		printf("Unflag_Proximity\r\n");
		//PS_LED_Status=0;		
		//PS_IO_OUT=0;						
	}	
}


 bool PL2219_MP_Initial_Config(void)
{

	uint8_t buffer[2] = {0};

	HAL_Delay(100);

	int count_down = 10;

	while (1)
	{
		/* code */

		if(wireReadDataByte(0x04,&buffer[0])){
				break;
		}
			HAL_Delay(10);
			count_down--;
			if(count_down <=0){
				printf("2219 init fail\r\n");
				return false;	
			}
	}
	


	if( (!wireReadDataByte(0x04,&buffer[1]))  ){

		return false;
	 }  
	 if( (!wireReadDataByte(0x05,&buffer[1])) ){

		return false;
	 }




	 if( ! wireWriteDataByte(0x00,0xA0) ){
		//return false;
	 } 			
	if( ! wireWriteDataByte(0x04,0x79)){ 		//0x04寄存器的高四位配置工作周期(0~F)，周期越长功耗越低，低四位配置曝光时间(0~F)曝光时间越长感应距离越远
		//return false;
	 } 
	if( ! wireWriteDataByte(0x05,0x00)){    	
		//return false;
	 } 

	 if( (!wireReadDataByte(0x04,&buffer[0])) ){

		//return false;
	 }  
	 if( (!wireReadDataByte(0x05,&buffer[1])) ){

		//return false;
	 }
	

		printf("\r\n read reg %02x,%02x\r\n",buffer[0],buffer[1]);

		
	 if( (buffer[0] != 0x79)  && (buffer[1] != 0x00) ){

	    //return false;
	 }




	if( ! wireWriteDataByte(0x06,0x4A)){   	
		//return false;
	 } 
	if( ! wireWriteDataByte(0x07,0x91)){ 	 		//LED驱动电流配置:0x91=5mA,0xB1=10mA,0xD1=15mA,0xF1=20mA //0x07=0x61时，电流直推模式，LED驱动电流由限流电阻决定
		//return false;
	 } 
	if( ! wireWriteDataByte(0x0D,0x08)){    	
		//return false;
	 } 
	//delay_1ms(5); 
	HAL_Delay(5);

	HAL_Delay(150);

	flag_Proximity=0;	
	No_Hover_CNT=0;    	//无接近感应计数
	Hover_CNT=0;        //有接近感应计数        
	PL2219_MP_Image_Dark_Deal();
	HAL_Delay(5);

	Diff_Pixel0=Get_PL2219_MP_Image_Diff();  				//SET environment initial value 


	// Diff_Pixel0 = 1600;
	// Diff_Pixel_Set_TH=Diff_Pixel0+TH_Diff_H; 
	// Diff_Pixel0 = 50;
	// Diff_Pixel_Set_TL=Diff_Pixel0+TH_Diff_L;

	printf("Diff_Pixel0 %d,H %d,L %d\r\n",Diff_Pixel0,Diff_Pixel_Set_TH,Diff_Pixel_Set_TL);

	uint8_t is_busy = 0;
	 wireWriteDataByte(0x00,0xA0); 	 						//Power ON使能传感器               
	 
	 do	{
	 	wireReadDataByte(0x00,&is_busy); 				//等待PL2219-MP芯片运算完成：读取0x00寄存器判断Bit1(Ls_busy)是否等于0
	 }
	 while(is_busy & 0x02);

	return true;

}

void PL2219_Init(){
	
	
	
	__HAL_RCC_GPIOA_CLK_ENABLE();                          /* Enable GPIOA clock */

	GPIO_InitTypeDef  gpio_init;
	gpio_init.Pin = GPIO_PIN_1|GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7;
	gpio_init.Mode = GPIO_MODE_OUTPUT_OD;            /* Push-pull output */
	gpio_init.Pull = GPIO_PULLUP;                    /* Enable pull-up */
	gpio_init.Speed = GPIO_SPEED_FREQ_LOW;          /* GPIO speed */  
	HAL_GPIO_Init(GPIOA, &gpio_init);
	HAL_GPIO_WritePin(GPIOA,GPIO_PIN_1|GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7,INIT_ALL_PIN_LEVEL);

	
	// while(1)
	
		{

			for(int i = 0;i < 4;i++){
				
					//HAL_Delay(200);
					PL2219.i2c_id = i;
					PL2219.PL2219[i].id_num = i;
					PL2219.PL2219[i].is_init  = PL2219_MP_Initial_Config();
					PL2219.PL2219[i].status = false;
					PL2219.PL2219[i].last_status = false;
					printf("PL2219 num %d is %d\r\n",i,PL2219.PL2219[i].is_init);
			
			}
			
		}

}







void PL2219_Init_State(_PL2219_t *PL2219) {
    if (!PL2219->state_is_init) {
        PL2219->state_is_init = true;

        // 直接悬空退出检查状态
        if (PL2219->scale_proximity < AIR_PROXIMITY_THREASOLD) {
            flag_Proximity = 0;
            PL2219->current_state = STATE_IN_AIR;
            PL2219->last_state = STATE_IN_AIR;
            PL2219->Set_Out(PL2219->pins, Trig_CLIFF_PIN_LEVEL);
			last_scale_proximit[PL2219->id_num] = PL2219->scale_proximity;
            printf("Init goto AIR_STATE\r\n");
            return;
        } else if (PL2219->scale_proximity < BLACK_PROXIMITY_THREASOLD) {
            flag_Proximity = 1;
            PL2219->current_state = STATE_BLACK_SURFACE;
            PL2219->last_state = STATE_BLACK_SURFACE;
            PL2219->Set_Out(PL2219->pins, UnTrig_CLIFF_PIN_LEVEL);
			last_scale_proximit[PL2219->id_num] = PL2219->scale_proximity;
            printf("Init goto NEAR_BLACK_OBJECT_STATE\r\n");
            return;
        } else {
            flag_Proximity = 1;
            PL2219->current_state = STATE_NEAR_OBJECT;
            PL2219->last_state = STATE_NEAR_OBJECT;
            PL2219->Set_Out(PL2219->pins, UnTrig_CLIFF_PIN_LEVEL);
			last_scale_proximit[PL2219->id_num] = PL2219->scale_proximity;
            printf("Init goto NEAR_WHILTE_OBJECT_STATE\r\n");
            return;
        }
    }
}



void PL2219_Task_Entry(_PL2219_t *PL2219) {


	#if 0
    PL2219_Init_State(PL2219);

    // 直接悬空退出检查状态
    if (PL2219->scale_proximity < AIR_PROXIMITY_THREASOLD) {
        flag_Proximity = 0;
        PL2219->current_state = STATE_IN_AIR;
        PL2219->last_state = STATE_IN_AIR;
		PL2219->last_scale_proximit = 0;
        PL2219->Set_Out(PL2219->pins, Trig_CLIFF_PIN_LEVEL);




		PL2219->is_deal1 = false;
		PL2219->count1 = 0;

		PL2219->is_deal = false;
		PL2219->count = 0;
        printf("1 goto AIR_STATE\r\n");
        return;
    }

    // 悬空状态  ----->  白色桌面
    if ((PL2219->last_state == STATE_IN_AIR) &&
        (PL2219->scale_proximity > AIR_PROXIMITY_THREASOLD) &&
        (PL2219->scale_proximity > WHILE_PROXIMITY_THREASOLD)) {

        flag_Proximity = 1;
        PL2219->current_state = STATE_NEAR_OBJECT;
		PL2219->last_scale_proximit = PL2219->scale_proximity;
        PL2219->last_state = STATE_NEAR_OBJECT;
        PL2219->Set_Out(PL2219->pins, UnTrig_CLIFF_PIN_LEVEL);
        printf("2 goto NEAR_WHILTE_OBJECT_STATE\r\n");
        return;
    }

    // 悬空状态  ----->  黑色桌面
    if ((PL2219->last_state == STATE_IN_AIR) &&
        (PL2219->scale_proximity > AIR_PROXIMITY_THREASOLD) &&
        (PL2219->scale_proximity < BLACK_PROXIMITY_THREASOLD)) {

        flag_Proximity = 1;
        PL2219->current_state = STATE_BLACK_SURFACE;
		PL2219->last_scale_proximit = PL2219->scale_proximity;
        PL2219->last_state = STATE_BLACK_SURFACE;
        PL2219->Set_Out(PL2219->pins, UnTrig_CLIFF_PIN_LEVEL);
        printf("3 goto NEAR_BLACK_OBJECT_STATE\r\n");
        return;
    }

    // 白色桌面  ----->  远离白色桌面状态
    if ((PL2219->last_state == STATE_NEAR_OBJECT) &&
        (PL2219->scale_proximity > AIR_PROXIMITY_THREASOLD) /*&&
        (PL2219->scale_proximity < (WHILE_PROXIMITY_THREASOLD - WHILE_MOVE_PROXIMITY_THREASOLD))*/) {

			//printf("if 4\r\n");
			uint8_t is_mutation = 0x00;
			//突然白色桌面->悬空状态 值突变
			if((is_mutation_thresold(PL2219->id_num,MUTATION_THREASOLD,&is_mutation))){

				printf("%d is_mutation data %d\r\n",PL2219->id_num,is_mutation);
				unsigned long current_time = HAL_GetTick();  // 获取当前时间

					//如果发生突变且冷却时间已过去
					if (!is_mutation_detected[PL2219->id_num] && (current_time - last_mutation_time[PL2219->id_num] > MUTATION_COOLDOWN_TIME)) {
							is_mutation_detected[PL2219->id_num] = true;  // 标记突变已处理
							mutation_count[PL2219->id_num]++;  // 增加突变计数器
							PL2219->Set_Out(PL2219->pins, Trig_CLIFF_PIN_LEVEL);   //触发悬崖
							PL2219->last_scale_proximit = PL2219->scale_proximity;
							//printf("Sensor %d mutation detected, Hold On.\n", PL2219->id_num);
							last_mutation_time[PL2219->id_num] = current_time;  // 更新最后突变时间



							//等待稳定
							do{

									printf("Sensor %d still in Hold On.\n", PL2219->id_num);
									last_scale_proximit[PL2219->id_num] = PL2219->scale_proximity;
									PL2219->is_stable  = false;
									HAL_Delay(5);     

							}while(!is_stable_for_time(PL2219->id_num));

							// 在 Hold On 状态下检查数据稳定性
							is_mutation_detected[PL2219->id_num] = false;  // 清空标记突变已处理
							mutation_count[PL2219->id_num] = 0;  	       // 清空突变计数器
							flag_Proximity = 0;
							PL2219->current_state = STATE_NEAR_OBJECT_AIR;
							PL2219->last_scale_proximit = PL2219->scale_proximity;
							PL2219->last_state = STATE_NEAR_OBJECT_AIR;
							
							printf("4 goto NEAR_WHILTE_AIR_STATE\r\n");
							return;
					}
			
			}
	}



// 远离白色桌面状态  ----->  白色桌面状态


#if 1



if ((PL2219->last_state == STATE_NEAR_OBJECT_AIR) &&
    (PL2219->scale_proximity > AIR_PROXIMITY_THREASOLD)) {

	//等待稳定
	while(!is_stable_for_time(PL2219->id_num)){
				printf("5 Sensor %d still in Hold On.\n", PL2219->id_num);
				last_scale_proximit[PL2219->id_num] = PL2219->scale_proximity;
				PL2219->is_stable  = false;
				HAL_Delay(5);     
	}
		//printf("if 4\r\n");
			uint8_t is_mutation = 0x00;
			//突然白色桌面->悬空状态 值突变
			if((is_mutation_thresold(PL2219->id_num,MUTATION_THREASOLD,&is_mutation))){

				printf("%d is_mutation data %d\r\n",PL2219->id_num,is_mutation);
				unsigned long current_time = HAL_GetTick();  // 获取当前时间

					//如果发生突变且冷却时间已过去
					if (!is_mutation_detected[PL2219->id_num] && (current_time - last_mutation_time[PL2219->id_num] > MUTATION_COOLDOWN_TIME)) {
							is_mutation_detected[PL2219->id_num] = true;  // 标记突变已处理
							mutation_count[PL2219->id_num]++;  // 增加突变计数器
							PL2219->Set_Out(PL2219->pins, UnTrig_CLIFF_PIN_LEVEL);   //不触发悬崖
							PL2219->last_scale_proximit = PL2219->scale_proximity;
							//printf("Sensor %d mutation detected, Hold On.\n", PL2219->id_num);
							last_mutation_time[PL2219->id_num] = current_time;  // 更新最后突变时间



							//等待稳定
							do{

									printf("5 Sensor %d still in Hold On.\n", PL2219->id_num);
									last_scale_proximit[PL2219->id_num] = PL2219->scale_proximity;
									PL2219->is_stable  = false;
									HAL_Delay(5);     

							}while(!is_stable_for_time(PL2219->id_num));

							// 在 Hold On 状态下检查数据稳定性
							is_mutation_detected[PL2219->id_num] = false;  // 清空标记突变已处理
							mutation_count[PL2219->id_num] = 0;  	       // 清空突变计数器
							flag_Proximity = 0;
							PL2219->current_state = STATE_NEAR_OBJECT;
							PL2219->last_scale_proximit = PL2219->scale_proximity;
							PL2219->last_state = STATE_NEAR_OBJECT;
							
							printf("5 goto STATE_NEAR_OBJECT\r\n");
							return;
					}
			
			}






}




#else


if ((PL2219->last_state == STATE_NEAR_OBJECT_AIR) &&
    (PL2219->scale_proximity > AIR_PROXIMITY_THREASOLD)) {

			uint8_t is_mutation = 0x00;

			 //突然白色桌面->悬空状态 值突变
			if((is_mutation_thresold(PL2219->id_num,MUTATION_THREASOLD,&is_mutation))){

				//printf("%d is_mutation data %d\r\n",PL2219->id_num,is_mutation);
            	unsigned long current_time = HAL_GetTick();  // 获取当前时间

				 //如果发生突变且冷却时间已过去
            if (!is_mutation_detected[PL2219->id_num] && (current_time - last_mutation_time[PL2219->id_num] > MUTATION_COOLDOWN_TIME)) {
                is_mutation_detected[PL2219->id_num] = true;  // 标记突变已处理
                mutation_count[PL2219->id_num]++;  // 增加突变计数器
                PL2219->is_deal = true;
                PL2219->last_scale_proximit = PL2219->scale_proximity;
                PL2219->status_update = true;
                printf("Sensor %d mutation detected, Hold On.\n", PL2219->id_num);
                last_mutation_time[PL2219->id_num] = current_time;  // 更新最后突变时间
            }
				// 使用 is_stable_for_time 判断传感器是否稳定
				// if(is_mutation){
				// 	 unsigned long current_time = HAL_GetTick();  // 获取当前时间
				// 	if (PL2219->is_deal == false) {
				// 		PL2219->is_deal = true;  // 启动持久状态
				// 		PL2219->hold_on = false;
				// 		PL2219->status_update = true;
				// 		PL2219->last_scale_proximit = PL2219->scale_proximity;
				// 		printf("5 %d Hold On NEAR_WHILTE_AIR_STATE\r\n",PL2219->id_num);
				// 	}
				// }
				
				
				
				//return;
			}

        // 在 Hold On 状态下检查数据稳定性
        if (PL2219->is_deal && !is_stable_for_time(PL2219->id_num)) {
            printf("Sensor %d still in Hold On.\n", PL2219->id_num);
			last_scale_proximit[PL2219->id_num] = PL2219->scale_proximity;
			PL2219->is_stable  = false;
            return;  // 如果数据未稳定，保持在 Hold On 状态
        }else if(PL2219->is_deal && is_stable_for_time(PL2219->id_num)){



					
			last_scale_proximit[PL2219->id_num] = PL2219->scale_proximity;
			is_mutation_detected[PL2219->id_num] = false;  // 清空标记突变已处理
			mutation_count[PL2219->id_num] = 0;  	       // 清空突变计数器
			PL2219->is_stable  = true;


			// 如果已经进入 Hold On 状态，且值发生较大变化，切换回白色桌面状态
			if (PL2219->is_deal) {

					uint8_t is_mutation = 0x00;
					// if
					//  (/*is_stable_for_time(PL2219->id_num)*/ 
					// 	(PL2219->is_stable == true) && 
					//     (
					// 	 /*(abs(PL2219->scale_proximity - PL2219->last_scale_proximit) > WHILE_MOVE_PROXIMITY_THREASOLD) ||*/
					// 	   (is_mutation_thresold(PL2219->id_num,WHILE_MOVE_PROXIMITY_THREASOLD,&is_mutation)))	 
					// ) 
						//if(PL2219->is_stable == true) 
					{
						printf("is_deal\r\n");
						if((is_mutation_thresold(PL2219->id_num,MUTATION_THREASOLD/5,&is_mutation))){


							if(is_mutation){

								PL2219->is_deal = false;  // 退出 Hold On 状态
								flag_Proximity = 1;
								PL2219->current_state = STATE_NEAR_OBJECT;
								PL2219->last_scale_proximit = PL2219->scale_proximity;
								PL2219->last_state = STATE_NEAR_OBJECT;
								PL2219->Set_Out(PL2219->pins, UnTrig_CLIFF_PIN_LEVEL);
								printf("5 %d goto NEAR_WHILTE_OBJECT_STATE\r\n",PL2219->id_num);
							}else{

									printf("is_mutation < 0\r\n");
							}

						}
					
					}

				PL2219->is_deal = false;  // 退出 Hold On 状态		

		}





			
		}
		// if ( (PL2219->is_deal == true) && (is_stable_for_time(PL2219->id_num)) ) {

		// 			printf("5 %d stable_for_time\r\n",PL2219->id_num);
		// 			PL2219->status_update = false;
		// 			//last_scale_proximit[PL2219->id_num] = PL2219->scale_proximity;

		// 			PL2219->is_stable = true;
		// }else if((PL2219->is_deal == true) && (!is_stable_for_time(PL2219->id_num)) ){

		// 			//printf("5 not stable_for_time\r\n");
		// 			last_scale_proximit[PL2219->id_num] = PL2219->scale_proximity;
		// 			PL2219->is_stable = false;
		// }
			
		
	



    // 判断是否接近白色桌面状态
    // if (PL2219->scale_proximity > (WHILE_PROXIMITY_THREASOLD - WHILE_MOVE_PROXIMITY_THREASOLD)) {
    // } 



	
	
	 if (PL2219->scale_proximity < BLACK_PROXIMITY_THREASOLD) {
        // 如果当前接近黑色桌面，直接跳转
        if (is_stable_for_time(PL2219->id_num)) {  // 稳定时间内切换到黑色桌面状态
            PL2219->is_deal = false;
            flag_Proximity = 1;
            PL2219->current_state = STATE_BLACK_SURFACE;
            PL2219->last_scale_proximit = PL2219->scale_proximity;
            PL2219->last_state = STATE_BLACK_SURFACE;
            PL2219->Set_Out(PL2219->pins, UnTrig_CLIFF_PIN_LEVEL);
            printf("5 goto STATE_BLACK_SURFACE\r\n");
        }
    }




		


    
    return;
}

#endif
  

    // 白色桌面状态  ----->  黑色桌面状态
    if ((PL2219->last_state == STATE_NEAR_OBJECT) &&
        (PL2219->scale_proximity > AIR_PROXIMITY_THREASOLD) &&
        (PL2219->scale_proximity < BLACK_PROXIMITY_THREASOLD)) {

        flag_Proximity = 1;
        PL2219->current_state = STATE_BLACK_SURFACE;
        PL2219->last_scale_proximit = PL2219->scale_proximity;
        PL2219->last_state = STATE_BLACK_SURFACE;
        PL2219->Set_Out(PL2219->pins, UnTrig_CLIFF_PIN_LEVEL);
        printf("6 goto NEAR_BLACK_OBJECT_STATE\r\n");
        return;
    }

    // 黑色桌面状态  ----->  黑色桌面远离状态
    if ((PL2219->last_state == STATE_BLACK_SURFACE) &&
        (PL2219->scale_proximity > AIR_PROXIMITY_THREASOLD)) {

        if (PL2219->scale_proximity < BLACK_PROXIMITY_THREASOLD) {

            // 远离黑色桌面判断
            if ((PL2219->last_scale_proximit > PL2219->scale_proximity)) {

                uint8_t diff = 0;
                diff = PL2219->last_scale_proximit - PL2219->scale_proximity;

                if (diff > BLACK_MOVE_PROXIMITY_THREASOLD) {

                    flag_Proximity = 0;
                    PL2219->current_state = STATE_BLACK_SURFACE_AIR;
                    PL2219->last_state = STATE_BLACK_SURFACE_AIR;
                    PL2219->Set_Out(PL2219->pins, Trig_CLIFF_PIN_LEVEL);
                    printf("7 goto NEAR_BLACK_OBJECT_AIR_STATE\r\n");
                    return;
                }
            }

        } else { // 突然进入白色桌面

            flag_Proximity = 1;
            PL2219->current_state = STATE_NEAR_OBJECT;
            PL2219->last_state = STATE_NEAR_OBJECT;
            PL2219->Set_Out(PL2219->pins, UnTrig_CLIFF_PIN_LEVEL);
            printf("8 goto NEAR_WHILTE_OBJECT_STATE\r\n");
            return;
        }
    }
#else






		 if (PL2219->proximity < AIR_PROXIMITY_THREASOLD) {
		
				PL2219->Set_Out(PL2219->pins, Trig_CLIFF_PIN_LEVEL);

		 }	

		else if (PL2219->proximity < PROXIMITY_THREASOLD) {
		
				PL2219->Set_Out(PL2219->pins, Trig_CLIFF_PIN_LEVEL);
		 }
		 
		 	
		 else if (PL2219->proximity >= PROXIMITY_THREASOLD) {
		
				PL2219->Set_Out(PL2219->pins, UnTrig_CLIFF_PIN_LEVEL);
		 }


		//  if (PL2219->scale_proximity < AIR_PROXIMITY_THREASOLD) {
		
		// 		PL2219->Set_Out(PL2219->pins, Trig_CLIFF_PIN_LEVEL);

		//  }	

		// else if (PL2219->scale_proximity < PROXIMITY_THREASOLD) {
		
		// 		PL2219->Set_Out(PL2219->pins, Trig_CLIFF_PIN_LEVEL);
		//  }
		 
		 	
		//  else if (PL2219->scale_proximity > PROXIMITY_THREASOLD) {
		
		// 		PL2219->Set_Out(PL2219->pins, UnTrig_CLIFF_PIN_LEVEL);
		//  }


#endif
}



void PL2219_Task(){


	
	while(1){
		

			for(int i = 0;i<4;i++){
							
									if(PL2219.PL2219[i].is_init)
									{
									
											PL2219.i2c_id = i;
											//printf("PL2219.i2c_id = %d ",PL2219.i2c_id);
											PS_Detection_Deal(&PL2219.PL2219[i].scale_proximity, &PL2219.PL2219[i].proximity);
											// 添加当前传感器的采样数据
        							//add_sample(PL2219.PL2219[i].scale_proximity, i);
											//PS_App_OUT();
											PL2219_Task_Entry(&PL2219.PL2219[i]);
									}
					}
	}


}



/*******************************************************************************
 * Raw I2C Reads and Writes
 ******************************************************************************/

/**
 * @brief Writes a single byte to the I2C device (no register)
 *
 * @param[in] val the 1-byte value to write to the I2C device
 * @return True if successful write operation. False otherwise.
 */
static bool  wireWriteByte(uint8_t val)
{
    // Wire.beginTransmission(APDS9960_I2C_ADDR);
    // Wire.write(val);
    // if( Wire.endTransmission() != 0 ) {
    //     return false;
    // }


//    uint8_t data[1] = {0}; // ����һ����Ч�Ŀ���������
//    esp_err_t esp_err;
//    data[0] = val;
//    esp_err = i2c_master_write_to_device(I2C_MASTER_NUM, APDS9960_I2C_ADDR, data, sizeof(data), 1000 / portTICK_PERIOD_MS);   /*   SW reset   */

//     if (esp_err == ESP_OK) {

//        return true;
//     }
	
	
	  uint8_t data[1] = {0}; // ����һ����Ч�Ŀ���������
    data[0] = val;
		
	return I2C_Write(PL2219.i2c_id,PL2219_I2C_ADDR, 0x00,data,1);
    
    //return false;
}

/**
 * @brief Writes a single byte to the I2C device and specified register
 *
 * @param[in] reg the register in the I2C device to write to
 * @param[in] val the 1-byte value to write to the I2C device
 * @return True if successful write operation. False otherwise.
 */
static bool  wireWriteDataByte(uint8_t reg, uint8_t val)
{

		uint8_t data[1] = {0}; // ����һ����Ч�Ŀ���������
    data[0] = val;
		
	return I2C_Write(PL2219.i2c_id,PL2219_I2C_ADDR, reg,data,1);
    
}

/**
 * @brief Writes a block (array) of bytes to the I2C device and register
 *
 * @param[in] reg the register in the I2C device to write to
 * @param[in] val pointer to the beginning of the data byte array
 * @param[in] len the length (in bytes) of the data to write
 * @return True if successful write operation. False otherwise.
 */
static bool  wireWriteDataBlock(  uint8_t reg, 
                                        uint8_t *val, 
                                        unsigned int len)
{
   
	return I2C_Write(PL2219.i2c_id,PL2219_I2C_ADDR, reg,val,len);
}

/**
 * @brief Reads a single byte from the I2C device and specified register
 *
 * @param[in] reg the register to read from
 * @param[out] the value returned from the register
 * @return True if successful read operation. False otherwise.
 */
static bool  wireReadDataByte(uint8_t reg, uint8_t *val)
{
    return I2C_Read(PL2219.i2c_id,PL2219_I2C_ADDR, reg,val,1);
}

/**
 * @brief Reads a block (array) of bytes from the I2C device and register
 *
 * @param[in] reg the register to read from
 * @param[out] val pointer to the beginning of the data
 * @param[in] len number of bytes to read
 * @return Number of bytes read. -1 on read APDS9960_ERROR.
 */
static int  wireReadDataBlock(   uint8_t reg, 
                                        uint8_t *val, 
                                        unsigned int len)
{
    unsigned char i = 0;
    
    /* Indicate which register we want to read from */
    // if (!wireWriteByte(reg)) {
    //     return -1;
    // }
    /* Read block data */
    // Wire.requestFrom(APDS9960_I2C_ADDR, len);
    // while (Wire.available()) {
    //     if (i >= len) {
    //         return -1;
    //     }
    //     val[i] = Wire.read();
    //     i++;
    // }
//        uint8_t data[2] = {0}; // ����һ����Ч�Ŀ���������
//    esp_err_t esp_err;
//    data[0] = reg;
//        esp_err = i2c_master_write_read_device(I2C_MASTER_NUM,APDS9960_I2C_ADDR, data, 1, val, len, 1000 / portTICK_PERIOD_MS);
//        if (esp_err == ESP_OK) {

//            //val = data[1];
//            return len;
//             //printf("3 Found I2C device at address 0x%02X\n",data[1]);
//        }



	 return I2C_Read(PL2219.i2c_id,PL2219_I2C_ADDR, reg,val,len);


    return -1;
}